package nx.behaviors;
import nx.utils.*;

import javaclient3.*;
/**
 * Class implementing a Motor Schema (MS) used to navigate to a goal.
 * @author Alejandro Pustowka, Jonathan Hernandez
 */
public class NxMSGoToPoint extends NxBehavior{
    
    public static final int MODE_LINEAR = 0;
    public static final int MODE_EXPONENTIAL = 1;
    
    /**
     * Maximum distance to the goal. Beyond this distance the vectors magnitude
     * is the maximum (1).
     */
    private double maxDist = 4.0f;

    /**
     * Minimal distance to the goal.
     */
    private double minDist = 0.1f;

    /**
     * Exponential parameter, used for scaling the output vector when using
     * MODE_EXPONENTIAL.
     */
    private double exp_param = 3.0f;

    /**
     * The output of this behavior will be scaled in an exponential way or a
     * linear way (MODE_EXPONENTIAL or MODE_LINEAR, respectively).
     */
    private int atractorMode;
    
    
    
    //float maxVel= 0.8f;
    private boolean perceptionSchema = false;
    
    public Position2DInterface pos;
    NxBehavior goalGenerator;
    public Vector2D goalPoint;

    public NxMSGoToPoint(String name,Position2DInterface pos, Vector2D goalPoint, double maxDist, double minDist, int atractorMode) {
        super(name);
        this.pos = pos;
        this.goalPoint = goalPoint;
        this.atractorMode = atractorMode;

        this.setParamsDist(minDist, maxDist);
        this.setExponentialDistance((maxDist-minDist)/5);
    }
    
    public NxMSGoToPoint(String name,Position2DInterface pos, NxBehavior goalGen, double maxDist, double minDist, int atractorMode) {
        
        super(name);

        this.pos = pos;
        goalGenerator = goalGen;
        perceptionSchema = true;
        this.atractorMode = atractorMode;

        this.setParamsDist(minDist, maxDist);
        this.setExponentialDistance((maxDist-minDist)/5);
    }
    
    public Vector2D Value() {
        
        if(perceptionSchema)
        {
            goalPoint = (Vector2D)goalGenerator.Value();
        }

        //while(!pos.isDataReady());
        //get goalGen of robot
        //Pose2d pose = pos.read(RealPosition2d.READ_MODE_NO_WAIT);

        Vector2D myPos = new Vector2D(pos.getData().getPos());

        //print goalGen of robot
        //System.out.println("MSGOTOPOINT: Robot goalGen vector:");
        
        //myPos.print(myPos.MODE_XY);
        
        //Vector in direction to point
        Vector2D atractor = goalPoint.sub(myPos);
        
        //print attractor vector
        //System.out.println("MSGOTOPOINT: Attractor vector:");
        //atractor.print(atractor.MODE_MA);
        
        //Now define the magnitude based on the chosen mode 
        switch(atractorMode){
            
            case MODE_LINEAR:
                atractor = convertLinearMode(atractor);
                break;
            case MODE_EXPONENTIAL:
                atractor = convertExponentialMode(atractor);
                break;
        
        
        }
        //if(atractor.getMag()<10.0f)
            //atractor.setMag(0.0f);
        if(enabled)
            return atractor;
        else
            return new Vector2D();

    }

    /**
     * Set function for maxDist.
     * @param maxDist New value for maxDist.
     */
    public void setMaxDist(float maxDist)
    {
        this.maxDist = maxDist;
    }
    /**
     * Set function for minDist.
     * @param minDist New value for minDist.
     */
    public void setMinDist(float minDist)
    {
        this.minDist = minDist;
    }
    
    /**
     * Set function that combines minDist and maxDist.
     * @param minDist New value for minDist.
     * @param maxDist New value for maxDist.
     */
    public void setParamsDist(double minDist, double maxDist)
    {
        this.maxDist = maxDist;
        this.minDist = minDist;
    }

    /**
     * The resulting vector is minimized in an exponential way.
     * @param atractor Output of this behavior.
     * @return New output scaled according to an exponential function.
     */
    private Vector2D convertExponentialMode(Vector2D atractor){
        
        float x = (float)atractor.getMag();
        
        //if object is in the closest area, then shutdown atractor
        if(x<=minDist)
            return new Vector2D(0,0,Vector2D.MODE_XY);
        else{
            float newMag =(float)( 1 - Math.exp(-exp_param*x));

            atractor.setMag(newMag);
        }
        //atractor.scale(maxVel);
        
        return atractor;
        
        
        
    
    }
    
    /**
     * The resulting vector is minimized in an linear way.
     * @param atractor Output of this behavior.
     * @return New output scaled according to a linear function.
     */
    private Vector2D convertLinearMode(Vector2D atractor){
        
        //If vector greater than influence area, then normalize, else scale and 
        //minimize according to distance to goalPoint
        
            if(atractor.getMag()>maxDist){
                atractor=atractor.normalize();
                //return atractor.scale(maxVel);
                return atractor;
                
            }
            else{
                if(atractor.getMag()<=minDist){
                    
                                 
                    return new Vector2D(0,0,Vector2D.MODE_XY);
                }                    
                else
                    //return atractor.scale(1/maxDist*maxVel);
                    return atractor.scale(1/maxDist);
            }
        
    
    }
    //distancia a la cual el vector de velocidad debe disminuir al 37% (0.37).
    public void setExponentialDistance(double x){
        
        exp_param = -Math.log(1 - Math.sqrt(2)/2.0f) / x;
        
        System.out.println(exp_param);
    
    }

    public void changeGoalPoint(Vector2D goalPoint)
    {
        this.goalPoint = goalPoint;
    }


}
